/***************************************************************************************************************************
 * command_to_tello.h
 * 作者：蓝蓝蓝笨笨
 * 主要功能：命令模块
 * 1.从TELLO_Driver 读取位置、速度等状态信息
 * 2.发布位置环控制信息，但是TELLO是通过控制速度CMD_VEL来控制移动，还需要作一个位置环模式
***************************************************************************************************************************/
#ifndef COMMAND_TO_ROTORS_H
#define COMMAND_TO_ROTORS_H

#include <ros/ros.h>
#include <bitset>
#include <math_utils.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>

#include <sensor_msgs/Imu.h>

#include <uav_drone_msgs/DroneState.h>
#include <uav_drone_msgs/AttitudeTarget_replace.h>
#include <uav_drone_msgs/PositionTarget_replace.h>
#include <uav_drone_msgs/AttitudeReference.h>
#include <eigen_conversions/eigen_msg.h>
#include <std_msgs/Empty.h>
#include <geometry_msgs/Twist.h>
using namespace std;

class command_to_tello
{
    public:
    //constructed function 1
    command_to_tello(void):
    command_nh("~")
    {
        command_nh.param<string>("mav_name", mav_name, "tello");
        pos_drone_fcu_target    = Eigen::Vector3d(0.0,0.0,0.0);
        vel_drone_fcu_target    = Eigen::Vector3d(0.0,0.0,0.0);
        accel_drone_fcu_target  = Eigen::Vector3d(0.0,0.0,0.0);
        q_fcu_target            = Eigen::Quaterniond(0.0,0.0,0.0,0.0);
        euler_fcu_target        = Eigen::Vector3d(0.0,0.0,0.0);
        rates_fcu_target        = Eigen::Vector3d(0.0,0.0,0.0);
        Thrust_target           = 0.0;

        takeoff_flag = 0;
        land_flag = 0;
        // 【订阅】无人机期望位置/速度/加速度 坐标系:ENU系
        position_target_sub = command_nh.subscribe<uav_drone_msgs::PositionTarget_replace>("/drone_pos_estimator/setpoint_raw/target_local", 10, &command_to_tello::pos_target_cb,this);
        // 【发布】控制指令
        set_pub = command_nh.advertise<geometry_msgs::Twist>("/tello/cmd_vel", 10);
        // 【发布起飞指令】
       tello_takeoff_pub = command_nh.advertise<std_msgs::Empty>("/tello/takeoff", 10);
        // 【发布降落指令】
       tello_land_pub = command_nh.advertise<std_msgs::Empty>("/tello/land", 10);


    }

    string mav_name;
    //Target pos of the drone [from fcu]
    Eigen::Vector3d pos_drone_fcu_target;
    //Target vel of the drone [from fcu]
    Eigen::Vector3d vel_drone_fcu_target;
    //Target accel of the drone [from fcu]
    Eigen::Vector3d accel_drone_fcu_target;
    //Target att of the drone [from fcu]
    Eigen::Quaterniond q_fcu_target;
    Eigen::Vector3d euler_fcu_target;
    Eigen::Vector3d rates_fcu_target;
    //Target thrust of the drone [from fcu]
    float Thrust_target;

    //判断当前是否为起飞（land）状态，后续也可读取TELLO上传状态
    bool takeoff_flag;
    bool land_flag;

    // takeoff
    void takeoff();

    //  land
    void land();

    //发送位置期望值至飞控（输入：期望xyz,期望yaw）
    void send_pos_setpoint(const Eigen::Vector3d& pos_sp, float yaw_sp);



    
    private:

        ros::NodeHandle command_nh;

        ros::Subscriber position_target_sub;
        ros::Publisher tello_takeoff_pub;
        ros::Publisher tello_land_pub;
        ros::Publisher set_pub;


        void pos_target_cb(const uav_drone_msgs::PositionTarget_replace::ConstPtr& msg)
        {
            pos_drone_fcu_target = Eigen::Vector3d(msg->position.x, msg->position.y, msg->position.z);

            vel_drone_fcu_target = Eigen::Vector3d(msg->velocity.x, msg->velocity.y, msg->velocity.z);

            accel_drone_fcu_target = Eigen::Vector3d(msg->acceleration_or_force.x, msg->acceleration_or_force.y, msg->acceleration_or_force.z);
        }

};

void command_to_tello::takeoff()
{
    std_msgs::Empty takeoff_msg;
    // 发布takeoff话题即可触发起飞，需要注意起飞只能有一次，但是降落后可以重新起飞
    // 当前程序中还未对起飞只能有一次作出限制
    tello_takeoff_pub.publish(takeoff_msg);
}

void command_to_tello::land()
{
    std_msgs::Empty land_msg;
    // 发布land话题即可触发降落，需要注意降落只能有一次，但是起飞后可以重新降落
    // 当前程序中还未对起飞只能有一次作出限制
    tello_land_pub.publish(land_msg);
}

//发送位置期望值至飞控（输入：期望xyz,期望yaw）
void command_to_tello::send_pos_setpoint(const Eigen::Vector3d& pos_sp, float yaw_sp)
{
    geometry_msgs::Twist cmd_vel_set;
    //设置定点飞行的期望位置点
    cmd_vel_set.linear.x = pos_sp[0];
    cmd_vel_set.linear.y = pos_sp[1];
    cmd_vel_set.linear.z = pos_sp[2];
    cmd_vel_set.angular.z = yaw_sp;
    cmd_vel_set.angular.x = 0;
    cmd_vel_set.angular.y = 0;
    // 检查飞控是否收到控制量
    cout <<">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>command_to_tello<<<<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;
    cout << "Pos_target [X Y Z] : " << cmd_vel_set.linear.x << " [ P ] "<< cmd_vel_set.linear.y<<" [ P] "<<cmd_vel_set.linear.z<<" [ P ] "<<endl;
    cout << "Pos_target [YAW] : "<< cmd_vel_set.angular.z<<" [ P ] "<<endl;
    cout <<">>>>>>>>>>>>>>>>>>only see orientation(z and w), other num is zero [can web search]<<<<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;
    set_pub.publish(cmd_vel_set);
}
#endif


